//
// Created by qzj on 2021/11/3.
//

#include "models/IPM/IPManager.h"
#include "sensor_data/four_images_data.hpp"
#include "tools/file_manager.hpp"
#include <cmath>
#include "glog/logging.h"
#include "CommonFunc.h"
#include "global_defination/global_defination.h"

using namespace std;

namespace avp_slam {

    IPManager::IPManager(ros::NodeHandle &nh) : nh_(nh) {
        K = cv::Mat();
        ipm_save_dir = WORK_SPACE_PATH + "/dataset/ipm/";
        FileManager::CreateDirectory(ipm_save_dir);
        time_ipm0 = std::make_shared<TicToc>("ipm_height", true);
        camera_info_ = nh.subscribe<sensor_msgs::CameraInfo>("/robot_1/camera0/depth/camera_info", 100,
                                                             &IPManager::cmrInfoCallback, this);
    }

    void IPManager::cmrInfoCallback(const sensor_msgs::CameraInfoConstPtr &cmr_info) {
        K = cv::Mat::zeros(3, 3, CV_32FC1);
        K.at<float>(0, 0) = cmr_info->K[0];
        K.at<float>(0, 2) = cmr_info->K[2];
        K.at<float>(1, 1) = cmr_info->K[4];
        K.at<float>(1, 2) = cmr_info->K[5];
        K.at<float>(2, 2) = cmr_info->K[8];
    }

    cv::Mat IPManager::Concat(const FourImagesData &fourImagesData) {

        int width = fourImagesData.colorFront.cols;
        int height = fourImagesData.colorFront.rows;

        cv::Mat imgIPM0 = cv::Mat::zeros(height*2, width*2, CV_8UC3);
        LOG(INFO)<<"colorFront"<<endl;
        fourImagesData.colorFront.copyTo(imgIPM0.rowRange(0, height).colRange(0, width));
        LOG(INFO)<<"colorLeft"<<endl;
        fourImagesData.colorLeft.copyTo(imgIPM0.rowRange(0, height).colRange(width, width*2));
        LOG(INFO)<<"colorBack"<<endl;
        fourImagesData.colorBack.copyTo(imgIPM0.rowRange(height, height*2).colRange(width, width*2));
        LOG(INFO)<<"colorRight"<<endl;
        fourImagesData.colorRight.copyTo(imgIPM0.rowRange(height, height*2).colRange(0, width));

        return imgIPM0.clone();
    }
    cv::Mat IPManager::DoIPM(const FourImagesData &fourImagesData) {

        if (!K.rows) {
            LOG(INFO) << "No camera parameters!" << endl;
            return cv::Mat();
        } else {
            vector<cv::Mat> imgIPM_V;
            cv::Mat imgIPM0 = cv::Mat::zeros(500, 500, CV_8UC3);
            cv::Mat K_ivt;
            cv::invert(K, K_ivt);
            cv::Mat imgIPM;

//            基于相机高度的方案
            time_ipm0->tic();
            LOG(INFO)<<"partIPM"<<endl;
            partIPM(K_ivt, imgIPM0, fourImagesData.colorFront, 0);
            partIPM(K_ivt, imgIPM0, fourImagesData.colorLeft, -1.5707963267949);
            partIPM(K_ivt, imgIPM0, fourImagesData.colorBack, 3.141592653589793);
            partIPM(K_ivt, imgIPM0, fourImagesData.colorRight, 1.5707963267949);
            time_ipm0->toc();

//            ostringstream buffer;
//            buffer << ipm_save_dir << time << ".jpg";
//            string str = buffer.str();
//            cv::imwrite(str,imgIPM0);
            return imgIPM0.clone();
        }
    }

//camera height
    void IPManager::partIPM(const cv::Mat &K_ivt, cv::Mat &IPM, const cv::Mat &color, float rotation_z) {
        float fx_inv = K_ivt.at<float>(0, 0);
        float cx_inv = K_ivt.at<float>(0, 2);
        float i_ivt = K_ivt.at<float>(1, 0), j_ivt = K_ivt.at<float>(1, 1), k_ivt = K_ivt.at<float>(1, 2);

        float x_cam, y_cam;
        float proportion;
        float x_camT, y_camT;
        float ipm_x_roted, ipm_y_roted;
        float cosz = cos(rotation_z), sinz = sin(rotation_z);
        int ipm_u, ipm_v, ipm_width_half, ipm_height_half;
        ipm_width_half = IPM.cols / 2;
        ipm_height_half = IPM.rows / 2;

        for (int v = 0; v < color.rows; v++) {
            for (int u = 0; u < color.cols; u++) {
                y_cam = i_ivt * u + j_ivt * v + k_ivt;
                if (y_cam > 0.2) {
                    x_cam = fx_inv * u + cx_inv;

                    proportion = 60.3 / y_cam;

                    y_camT = -1 * proportion * x_cam;
                    x_camT = proportion;

                    ipm_x_roted = x_camT * cosz + y_camT * sinz;
                    ipm_y_roted = -1 * x_camT * sinz + y_camT * cosz;

//                    LOG(INFO)<<"1"<<endl;
                    ipm_u = ipm_height_half - ipm_y_roted;
                    ipm_v = ipm_width_half - ipm_x_roted;
                    if(ipm_u <= 0 || ipm_u >= IPM.cols || ipm_v <= 0 || ipm_v >= IPM.rows)
                        continue;

//                    LOG(INFO)<<"2: "<<ipm_u<<" "<<ipm_v<<" "<<u<<" "<<v<<endl;
//                    cv::mat.at(v,u)行列
                    IPM.at<cv::Vec3b>(ipm_v, ipm_u)[0] = color.at<cv::Vec3b>(v, u)[2];
                    IPM.at<cv::Vec3b>(ipm_v, ipm_u)[1] = color.at<cv::Vec3b>(v, u)[1];
                    IPM.at<cv::Vec3b>(ipm_v, ipm_u)[2] = color.at<cv::Vec3b>(v, u)[0];

                }
            }
        }
    }
// depth-based method
//    cv::Mat IPM::IPM(void) {
//        if(K.rows)
//        {
//            vector<cv::Mat> imgIPM_V;
//            cv::Mat imgIPM0 = cv::Mat::zeros(2600,2600,CV_8UC3);
//            cv::Mat imgIPM1 = cv::Mat::zeros(2600,2600,CV_8UC3);//store the IPM image.
//            cv::Mat K_ivt;
//            cv::invert(K,K_ivt);
//            cv::Mat imgIPM;
//
////            基于相机高度的方案
//            time_ipm0->tic();
//            partIPM(K_ivt,imgIPM0,images[1],0);
//            partIPM(K_ivt,imgIPM0,images[3],-1.5707963267949);
//            partIPM(K_ivt,imgIPM0,images[5],3.141592653589793);
//            partIPM(K_ivt,imgIPM0,images[7],1.5707963267949);
//            time_ipm0->toc();
//
////            基于深度图的方案
//            time_ipm1->tic();
//            partIPM(K_ivt,imgIPM1,images[0],images[1],0);
//            partIPM(K_ivt,imgIPM1,images[2],images[3],-1.5707963267949);
//            partIPM(K_ivt,imgIPM1,images[4],images[5],3.141592653589793);
//            partIPM(K_ivt,imgIPM1,images[6],images[7],1.5707963267949);
//            time_ipm1->toc();
//
//            imgIPM_V.push_back(imgIPM0);
//            imgIPM_V.push_back(imgIPM1);
//            hconcat(imgIPM_V, imgIPM);
//
//            ostringstream buffer;
//            buffer << ipm_save_dir << time << ".jpg";
//            string str = buffer.str();
//            cv::imwrite(str,imgIPM);
//            return imgIPM.clone();
//        }
//
//        LOG(INFO)<<"No camera parameters!"<<endl;
//        return cv::Mat();
//    }
//
}
